This script is necessary to recompute the parameters when kinematic apex data are "sub-frame" computed.
Requires IPython to run in "pylab" mode. If this is not the case, at least insert
from pylab import *
somewhere
In [1]:
%pylab qt
In [ ]:
%connect_info
In [2]:
import mutils.io as mio
import mutils.misc as mi
import os
import re
import mutils.wsviewer as wsv
ws = mio.saveable()
ws.wsv = wsv.WsView(ws, 'ws (main workspace)')
ws.subject = 6
ws.subject_doc = 'id of selected subject'
ws.ttype = 1
ws.ttype_doc = 'trial type (here: only 1 allowed)'
ws.datadir_out='data/2011-mmcl_mat/SLIP/new4/'
!mkdir -p 'data/2011-mmcl_mat/SLIP/new4/'
ws.datadir_out_doc = 'absolute path to store the recomputed SLIP data'
In [3]:
# load continous data
ws.k = mio.KinData(data_dir = 'data/2011-mmcl_mat/')
ws.k.selection = ['com_x', 'com_y', 'com_z']
ws.k.load(ws.subject, ws.ttype)
ws.k_doc = 'KinData object that handles the continuous data'
In [4]:
# find apices (at sampling rate accuracy)
all_apices = []
all_apexphases = []
for r in ws.k.raw_dat:
apices = []
apexphases = []
c = r['com'][:, 2]
phi = r['phi2'].squeeze()
rising = c[1] > c[0]
for idx, z in enumerate(c[1:]):
if rising and c[idx] > z:
apices.append(idx)
apexphases.append(phi[idx])
rising = False
if z >= c[idx]:
rising = True
all_apices.append(apices[:])
all_apexphases.append(apexphases[:])
# find nadirs (at sampling rate accuracy)
all_nadirs = []
all_nadirphases = []
for r in ws.k.raw_dat:
nadirs = []
nadirphases = []
c = r['com'][:, 2]
phi = r['phi2'].squeeze()
falling = c[1] < c[0]
for idx, z in enumerate(c[1:]):
if falling and c[idx] < z:
nadirs.append(idx)
nadirphases.append(phi[idx])
falling = False
if z <= c[idx]:
falling = True
all_nadirs.append(nadirs[:])
all_nadirphases.append(nadirphases[:])
ws.n_idcs = []
ws.n_idcs_doc = 'list of indices of (nearby) nadir events (one for each trial)'
ws.a_idcs = []
ws.a_idcs_doc = 'list of indices of (nearby) apex events (one for each trial)'
ws.n_phi = []
ws.n_phi_doc = 'list of phases of (nearby) nadir events (one for each trial)'
ws.a_phi = []
ws.a_phi_doc = 'list of phases of (nearby) apex events (one for each trial)'
nr = 0
for n, a, np, ap in zip(all_nadirs, all_apices, all_nadirphases, all_apexphases):
firstidx = 0
while n[firstidx] < a[0]:
firstidx += 1
#print "first:", firstidx, n[firstidx], a[firstidx], n[firstidx] - a[0]
ws.n_idcs.append(n[firstidx:][:])
ws.a_idcs.append(a)
ws.n_phi.append(np[firstidx:][:])
ws.a_phi.append(ap)
In [5]:
# interpolate apex and nadir values
ws.nx_idcs = []
ws.nx_idcs_doc = 'list of indices of (exact) nadir events (one for each trial)'
ws.ax_idcs = []
ws.ax_idcs_doc = 'list of indices of (exact) apex events (one for each trial)'
ws.nx_vals = []
ws.nx_vals_doc = 'list of heights of nadir events (one for each trial)'
ws.ax_vals = []
ws.ax_vals_doc = 'list of heights of apex events (one for each trial)'
for rep in range(len(ws.n_idcs)):
c = ws.k.raw_dat[rep]['com'][:, 2]
# start with nadir events
nx_idx = []
nx_val = []
for nidx in ws.n_idcs[rep]:
delta = 3
if nidx < 3 or nidx > 60000 - 3:
delta = 1
print "WARNING - nadir close to border detected"
pos, val = mi.get_minmax(c[nidx-delta:nidx+delta])
pos += nidx-delta
nx_idx.append(pos)
nx_val.append(val)
ws.nx_vals.append(nx_val)
ws.nx_idcs.append(nx_idx)
# continue with apex events
ax_idx = []
ax_val = []
for aidx in ws.a_idcs[rep]:
delta = 3
if aidx < 3 or aidx > 60000 - 3:
delta = 1
print "WARNING - apex close to border detected"
pos, val = mi.get_minmax(c[aidx-delta:aidx+delta])
pos += aidx-delta
if abs(val - c[aidx]) > .0001:
print aidx, ": val:", val, " orig:", c[aidx]
ax_idx.append(pos)
ax_val.append(val)
ws.ax_vals.append(ax_val)
ws.ax_idcs.append(ax_idx)
print "number of trials:", len(all_apices)
In [6]:
# sanity check: (almost) every nadir is AFTER the corresponding apex
if min(hstack([array(ws.nx_idcs[t])[:500] - array(ws.ax_idcs[t])[:500] for t in range(len(ws.nx_idcs))])) < 0:
raise ValueError, "This should not happen!"
In [11]:
import models.fitSlip as fit
reload(fit)
t = linspace(0, 240, 60000, endpoint=False)
dt = t[2] - t[1]
ka = ws.k.get_kin_apex(ws.a_phi) # list of 6-by-n apex states
# add belt speed. add this also to the dictionary that will be saved!
for rep in [5,]: #range(len(all_apices)):
vb = mean(ws.k.raw_frc[rep]['vb'])
# mass = ws.SlipData[rep].mass
mass = mean(ws.k.raw_frc[0]['fz_c']) / 9.81 # not very exact, but will be normalized anyway ...
#raise NotImplementedError('PHASES: calculate new phases from raw_dat!')
#phases = ws.SlipData[rep].phases
phi = ws.k.raw_dat[rep]['phi2'].squeeze()
mdp = mean(diff(phi))
phases = []
#orig = ws.SlipData[rep].ESLIP_params
ESLIP_params = []
SLIP_params = ESLIP_params # have a second name for the field: compatibility
# old: ws.SlipData[rep].SLIP_params # just copy to keep format intact
vx = []
y0 = []
vz = []
ymin = []
dE = []
T_exp = []
for step in range(len(all_apices[rep]) - 1):
T = (ws.ax_idcs[rep][step+1] - ws.ax_idcs[rep][step]) * dt
dp = (1. - mod(ws.ax_idcs[rep][step], 1))*(phi[ws.ax_idcs[rep][step]+1] - phi[ws.ax_idcs[rep][step]])
if dp > 4*mdp:
raise ValueError("Interpolation between two sampled phases gave too large value!")
phi0 = phi[ws.ax_idcs[rep][step]] + dp
IC = [ws.ax_vals[rep][step], ka[rep][4, step] + vb, ka[rep][3, step]] # [y0, vx, vz]
FS = [ws.ax_vals[rep][step+1], ka[rep][4, step+1] + vb, ka[rep][3, step+1]] # [y0, vx, vz]
E0 = mass * 9.81 * IC[0] + .5 * mass * (IC[1]**2 + IC[2]**2)
Ee = mass * 9.81 * FS[0] + .5 * mass * (FS[1]**2 + FS[2]**2)
dE_l = Ee - E0 # name "dE" already taken
dE.append(dE_l)
T_exp.append(T)
phases.append(phi0)
vx.append(IC[1])
y0.append(IC[0])
vz.append(IC[2])
ymin.append(ws.nx_vals[rep][step])
if False: # old code
# model- and step parameters
mp = { 'IC' : IC,
'm' : mass,
'dE' : dE_l}
sp = (ws.ax_vals[rep][step+1], T ,
ws.nx_vals[rep][step], ka[rep][3, step+1])
#Initial guess: take from "original", but modify according to modified means mean
if len(ESLIP_params) < 10:
x0 = orig[step, :]
x0[2] -= .01
else:
x0 = mean(vstack(ESLIP_params), axis=0) + (orig[step,:] - mean(orig, axis=0))
x0[2] -= .005
# check if starting condition is valid:
while x0[2] * sin(x0[1]) > IC[0]:
print "updated starting parameter"
x0[2] -= .01
if False: # original data are not taken into account at all!
#Initial guess: take from "original", but modify according to modified means mean
# NOTE: actually, x0[4] is the wrong value, but it's ignored in the calculation.
if len(ESLIP_params) < 10:
x0 = orig[step, :5]
x0[2] -= .01
else:
x0 = mean(vstack(ESLIP_params), axis=0) + (orig[step,:5] - mean(orig[:, :5], axis=0))
x0[2] -= .005
#fit.calcSlipParams3D2(
pars = fit.calcSlipParams3D2(IC, mass, array(FS), ws.nx_vals[rep][step], T) # P0 = x0)
#ESLIP_params.append( fit.calcSlipParams3D(sp,mp, x0[:4]), )
ESLIP_params.append( pars)
if not mod(step, 50):
print "solution found (step:", step, "rep:", rep, ")"
# store data
fn = ws.datadir_out + 'params3D_s{}t1r{}.dict'.format(ws.subject, rep+1)
ESLIP_params = vstack(ESLIP_params)
# use "old" format.
ESLIP_out = hstack([ESLIP_params[:, :4], zeros((ESLIP_params.shape[0], 2))])
dE = ESLIP_params[:,4]
dat = {
'SLIP_params' : SLIP_params,
'T_exp' : T_exp,
'phases' : phases,
'dE' : array(dE),
'P' : ESLIP_params, # hstack([vstack(ESLIP_params)[:, :4], array(dE)[:, newaxis]]),
'IC' : vstack([y0, vx, vz]).T,
'ESLIP_params' : ESLIP_params,
'mass' : mass,
'vx' : array(vx),
'y0' : array(y0),
'vz' : array(vz),
'ymin' : array(ymin),
'vb' : vb
}
mio.msave(fn, dat)
print "data stored"
print "done!"
In [12]:
mdp
Out[12]:
In [19]:
figure()
plot(phi,'.-')
Out[19]:
In [13]:
dp
Out[13]:
In [ ]:
raise NotImplementedError("STOP")
In [ ]:
# how to compute slip params later on